#!/usr/bin/env python3
import rospy
import numpy as np
import multiprocessing.shared_memory
import posix_ipc  # 新增导入
import io  # 确保导入 io


class SharedMemoryCommunicator:
    def __init__(self, name, size, message_type):
        self.name = name
        self.size = size
        self.message_type = message_type
        try:
            self.create_shm = False
            self.shm = multiprocessing.shared_memory.SharedMemory(name=self.name, create=False, size=self.size)
        except FileNotFoundError:
            self.create_shm = True
            self.shm = multiprocessing.shared_memory.SharedMemory(name=self.name, create=True, size=self.size)

        # 尝试独占创建信号量
        try:
            self.mutex = posix_ipc.Semaphore(f"{self.name}_mux", posix_ipc.O_CREAT | posix_ipc.O_EXCL, initial_value=1)
            self.mutex_created = True
        except posix_ipc.ExistentialError:
            self.mutex = posix_ipc.Semaphore(f"{self.name}_mux")
            self.mutex_created = False

    def read(self):
        msg = self.message_type()
        self.mutex.acquire()
        try:
            a = self.shm.buf[: self.size].tobytes()
            msg.deserialize(a)
        finally:
            self.mutex.release()
        return msg

    def write(self, msg):
        buf = io.BytesIO()
        msg.serialize(buf)
        bufData = bytearray(buf.getvalue())
        self.mutex.acquire()
        try:
            self.shm.buf[: len(bufData)] = bufData
        finally:
            self.mutex.release()

    def close(self):
        if self.create_shm:
            self.shm.close()
            self.shm.unlink()

        self.mutex.close()
        if self.mutex_created:
            try:
                self.mutex.unlink()
            except posix_ipc.ExistentialError:
                pass  # ��号量已被其他进程解除


if __name__ == "__main__":
    from std_msgs.msg import Header

    rospy.init_node("shared_memory_communicator", anonymous=True)
    cppServer = SharedMemoryCommunicator("cpp_server", 1024, Header)
    pythonServer = SharedMemoryCommunicator("python_server", 1024, Header)
    header = Header()
    header.frame_id = "python"
    while not rospy.is_shutdown():
        print("============================")
        print(cppServer.read())
        header.stamp = rospy.Time.now()
        pythonServer.write(header)
        rospy.sleep(0.5)
    cppServer.close()
    pythonServer.close()
